// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Quaternion.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System;
    using System.ComponentModel;
    using System.Runtime.Serialization;

    /// <summary>
    /// Quaternion class
    /// </summary>
    [DataContract]
    public struct Quaternion
    {
        /// <summary>
        /// X component
        /// </summary>
        [DataMember(Name = "X")]
        private double xcomponent;

        /// <summary>
        /// Y component
        /// </summary>
        [DataMember(Name = "Y")]
        private double ycomponent;

        /// <summary>
        /// Z component
        /// </summary>
        [DataMember(Name = "Z")]
        private double zcomponent;

        /// <summary>
        /// W component
        /// </summary>
        [DataMember(Name = "W")]
        private double wcomponent;

        /// <summary>
        /// Initializes a new instance of the Quaternion struct.
        /// </summary>
        /// <param name="x">X component</param>
        /// <param name="y">Y component</param>
        /// <param name="z">Z component</param>
        /// <param name="w">W component</param>
        public Quaternion(double x, double y, double z, double w)
        {
            this.xcomponent = x;
            this.ycomponent = y;
            this.zcomponent = z;
            this.wcomponent = w;
        }

        /// <summary>
        /// Initializes a new instance of the Quaternion struct.
        /// </summary>
        /// <param name="q">Quaternion to be copied</param>
        public Quaternion(Quaternion q)
        {
            this.xcomponent = q.xcomponent;
            this.ycomponent = q.ycomponent;
            this.zcomponent = q.zcomponent;
            this.wcomponent = q.wcomponent;
        }

        /// <summary>
        /// Initializes a new instance of the Quaternion struct from a homogenous transform.
        /// Translational elements of transform are ignored.
        /// </summary>
        /// <param name="ht">Homogenous transform to initialize quaternion from.</param>
        public Quaternion(Matrix4 ht)
        {
            this.xcomponent = 0.5 * ZeroPosSign(ht[2, 1] - ht[1, 2]) * Math.Sqrt(Math.Max(1 + ht[0, 0] - ht[1, 1] - ht[2, 2], 0.0));
            this.ycomponent = 0.5 * ZeroPosSign(ht[0, 2] - ht[2, 0]) * Math.Sqrt(Math.Max(1 - ht[0, 0] + ht[1, 1] - ht[2, 2], 0.0));
            this.zcomponent = 0.5 * ZeroPosSign(ht[1, 0] - ht[0, 1]) * Math.Sqrt(Math.Max(1 - ht[0, 0] - ht[1, 1] + ht[2, 2], 0.0));
            this.wcomponent = 0.5 * Math.Sqrt(Math.Max(1 + ht[0, 0] + ht[1, 1] + ht[2, 2], 0.0));
        } 

        /// <summary>
        /// Gets or sets x component
        /// </summary>
        [Description("The X component of the quaternion.")]
        public double X
        {
            get
            {
                return this.xcomponent;
            }

            set
            {
                this.xcomponent = value;
            }
        }

        /// <summary>
        /// Gets or sets y component
        /// </summary>
        [Description("The Y component of the quaternion.")]
        public double Y
        {
            get
            {
                return this.ycomponent;
            }

            set
            {
                this.ycomponent = value;
            }
        }

        /// <summary>
        /// Gets or sets z component
        /// </summary>
        [Description("The Z component of the quaternion.")]
        public double Z
        {
            get
            {
                return this.zcomponent;
            }

            set
            {
                this.zcomponent = value;
            }
        }

        /// <summary>
        /// Gets or sets w component
        /// </summary>
        [Description("The W component of the quaternion.")]
        public double W
        {
            get
            {
                return this.wcomponent;
            }

            set
            {
                this.wcomponent = value;
            }
        }

        /// <summary>
        /// Multiplies two quaternions.
        /// The order of the multiplication is important.
        /// Use Quaternion.Normalize to get unit quaternions
        /// </summary>
        /// <param name="lhs">Left hand side.</param>
        /// <param name="rhs">Right hand side.</param>
        /// <returns>The result of the multiplication.</returns>
        public static Quaternion operator *(Quaternion lhs, Quaternion rhs)
        {
            return Multiply(lhs, rhs);
        }

        /// <summary>
        /// Subtracts a quaternion from another quaternion.
        /// </summary>
        /// <param name="lhs">Left hand side.</param>
        /// <param name="rhs">Right hand side.</param>
        /// <returns>Result of the subtraction.</returns>
        public static Quaternion operator -(Quaternion lhs, Quaternion rhs)
        {
            return new Quaternion(
                lhs.xcomponent - rhs.xcomponent, 
                lhs.ycomponent - rhs.ycomponent, 
                lhs.zcomponent - rhs.zcomponent, 
                lhs.wcomponent - rhs.wcomponent);
        }

        /// <summary>
        /// Add a quaternion to another quaternion.
        /// </summary>
        /// <param name="lhs">Left hand side.</param>
        /// <param name="rhs">Right hand side.</param>
        /// <returns>Result of the addition.</returns>
        public static Quaternion operator +(Quaternion lhs, Quaternion rhs)
        {
            return new Quaternion(
                lhs.xcomponent + rhs.xcomponent,
                lhs.ycomponent + rhs.ycomponent,
                lhs.zcomponent + rhs.zcomponent,
                lhs.wcomponent + rhs.wcomponent);
        }

        /// <summary>
        /// Get the magnitude of the quaternion
        /// </summary>
        /// <param name="q">The quaternion to use</param>
        /// <returns>The magnitude</returns>
        public static double Length(Quaternion q)
        {
            return
                Math.Sqrt(
                    (q.xcomponent * q.xcomponent) + (q.ycomponent * q.ycomponent) + (q.zcomponent * q.zcomponent)
                    + (q.wcomponent * q.wcomponent));
        }

        /// <summary>
        /// Normalize a quaternion to a unit quaternion
        /// </summary>
        /// <param name="q">The quaternion to use</param>
        /// <returns>Normalized quaternion</returns>
        public static Quaternion Normalize(Quaternion q)
        {
            double mag = Length(q);
            Quaternion unitq = new Quaternion();

            if (mag != 0.0)
            {
                unitq.xcomponent = q.xcomponent / mag;
                unitq.Y = q.ycomponent / mag;
                unitq.Z = q.zcomponent / mag;
                unitq.W = q.wcomponent / mag;
            }
            else
            {
                unitq.xcomponent = q.xcomponent;
                unitq.ycomponent = q.ycomponent;
                unitq.zcomponent = q.zcomponent;
                unitq.wcomponent = q.wcomponent;
            }

            return unitq;
        }

        /// <summary>
        /// Create a quaternion given axis and angle
        /// </summary>
        /// <param name="x">X component of axis</param>
        /// <param name="y">Y component of axis</param>
        /// <param name="z">Z component of axis</param>
        /// <param name="angle">Angle in radians</param>
        /// <returns>Quaternion representing given axis angle rotation</returns>
        public static Quaternion FromAxisAngle(double x, double y, double z, double angle)
        {
            double qx, qy, qz, qw;

            qw = Math.Cos(angle / 2.0);
            qx = x * Math.Sin(angle / 2.0);
            qy = y * Math.Sin(angle / 2.0);
            qz = z * Math.Sin(angle / 2.0);

            Quaternion q = new Quaternion(qx, qy, qz, qw);

            return Normalize(q);
        }

        /// <summary>
        /// Convert an AxisAngle to a quaternion
        /// </summary>
        /// <param name="a">The axis angle to convert</param>
        /// <returns>Converted quaternion</returns>
        public static Quaternion FromAxisAngle(AxisAngle a)
        {
            AxisAngle unitA = new AxisAngle(a);
            if (a.Axis.Length() > 1.0)
            {
                unitA.Axis = Vector3.Normalize(a.Axis);
            }

            return FromAxisAngle(unitA.Axis.X, unitA.Axis.Y, unitA.Axis.Z, unitA.Angle);
        }

        /// <summary>
        /// Return the Quaternion that rotates inVec to outVec
        /// </summary>
        /// <param name="inVec">Input vector</param>
        /// <param name="outVec">Rotated vector</param>
        /// <returns>Quaternion for the rotation</returns>
        public static Quaternion QuaternionFromVectors(Vector3 inVec, Vector3 outVec)
        {
            Vector3 inVecNorm = Vector3.Normalize(inVec);
            Vector3 outVecNorm = Vector3.Normalize(outVec);
            Quaternion rot;

            double cosAng = Vector3.Dot(inVecNorm, outVecNorm);
            if (Math.Abs(cosAng - 1.0) < MathConstants.ErrorEpsilon)
            {
                rot = new Quaternion(0.0, 0.0, 0.0, 1.0);
            }
            else if (Math.Abs(cosAng + 1.0) < MathConstants.ErrorEpsilon)
            {
                // 180' rotation - degenerate case there are many solutions so can pick amongst many candidates
                // Here we choose a principal axis if it is approx orthogonal to the vecs otherwise create
                // an orthogonal axis
                Vector3 diff = inVec - outVec;
                Vector3 axis;

                if (Math.Abs(diff.X) < MathConstants.ErrorEpsilon)
                {
                    axis = new Vector3(1, 0, 0);
                }
                else if (Math.Abs(diff.Y) < MathConstants.ErrorEpsilon)
                {
                    axis = new Vector3(0, 1, 0);
                }
                else if (Math.Abs(diff.Z) < MathConstants.ErrorEpsilon)
                {
                    axis = new Vector3(0, 0, 1);
                }
                else
                {
                    // Create an orthogonal axis (that is axis dot diff = 0)
                    axis = new Vector3((-diff.Y - diff.Z) / diff.X, 1, 1);
                }

                rot = Quaternion.FromAxisAngle(axis.X, axis.Y, axis.Z, Math.PI);
            }
            else
            {
                Vector3 axis = Vector3.Cross(inVec, outVec);
                axis = Vector3.Normalize(axis);
                double ang = Math.Acos(Math.Min(Math.Max(cosAng, -1.0), 1.0));
                rot = Quaternion.FromAxisAngle(axis.X, axis.Y, axis.Z, ang);
            }

            rot = Quaternion.Normalize(rot);

            return rot;
        }

        /// <summary>
        /// Convert a quaternion to an AxisAngle
        /// </summary>
        /// <param name="q">The quaternion to convert</param>
        /// <returns>The converted axis angle</returns>
        public static AxisAngle ToAxisAngle(Quaternion q)
        {
            // quarterion for rotation should be around unit quaternion
            if (Math.Abs(Length(q) - 1.0) > MathConstants.ErrorEpsilon)
            {
                throw new ArgumentException("Quaternion for rotation should be a unit quaternion");
            }

            AxisAngle a = new AxisAngle();

            if (q.X == 0 && q.Y == 0 && q.Z == 0 && q.W == 0)
            {
                throw new ArgumentException("Invalid quaternion. X, Y, Z and W are all 0");
            }

            a.Angle = Math.Acos(q.W) * 2.0;
            double s = Math.Sqrt(1.0 - (q.W * q.W));
            Vector3 axisResult = new Vector3();

            // avoid division by 0
            if (s == 0.0)
            {
                axisResult.X = 0.0;
                axisResult.Y = 0.0;
                axisResult.Z = 0.0;
            }
            else
            {
                axisResult.X = q.xcomponent / s;
                axisResult.Y = q.ycomponent / s;
                axisResult.Z = q.zcomponent / s;
            }

            a.Axis = axisResult;
            return a;
        }

        /// <summary>
        /// Multiply q1 by q2. The order of the multiplication is important.
        /// Use Quaternion.Normalize to get unit quaternions
        /// </summary>
        /// <param name="q1">Left side</param>
        /// <param name="q2">Right side</param>
        /// <returns>Product quaternion</returns>
        public static Quaternion Multiply(Quaternion q1, Quaternion q2)
        {
            Quaternion q = new Quaternion();
            q.wcomponent = (q1.wcomponent * q2.wcomponent) - (q1.xcomponent * q2.xcomponent)
                           - (q1.ycomponent * q2.ycomponent) - (q1.zcomponent * q2.zcomponent);
            q.xcomponent = (q1.wcomponent * q2.xcomponent) + (q1.xcomponent * q2.wcomponent)
                           + (q1.ycomponent * q2.zcomponent) - (q1.zcomponent * q2.ycomponent);
            q.ycomponent = (q1.wcomponent * q2.ycomponent) - (q1.xcomponent * q2.zcomponent)
                           + (q1.ycomponent * q2.wcomponent) + (q1.zcomponent * q2.xcomponent);
            q.zcomponent = (q1.wcomponent * q2.zcomponent) + (q1.xcomponent * q2.ycomponent)
                           - (q1.ycomponent * q2.xcomponent) + (q1.zcomponent * q2.wcomponent);
            return q;
        }

        /// <summary>
        /// Returns the conjugate of a Quaternion
        /// </summary>
        /// <param name="q">Source Quaternion</param>
        /// <returns>The conjugate of a Quaternion</returns>
        public static Quaternion Conjugate(Quaternion q)
        {
            Quaternion result = q;
            result.X *= -1.0;
            result.Y *= -1.0;
            result.Z *= -1.0;
            return result;
        }

        /// <summary>
        /// Returns the inverse of a Quaternion
        /// </summary>
        /// <param name="q">Source Quaternion</param>
        /// <returns>The inverse of the Quaternion</returns>
        public static Quaternion Inverse(Quaternion q)
        {
            double invMag = 1.0
                            /
                            ((q.xcomponent * q.xcomponent) + (q.ycomponent * q.ycomponent)
                             + (q.zcomponent * q.zcomponent) + (q.wcomponent * q.wcomponent));
            return new Quaternion(
                -invMag * q.xcomponent, -invMag * q.ycomponent, -invMag * q.zcomponent, invMag * q.wcomponent);
        }

        /// <summary>
        /// Rotates a vector by a quaternion
        /// </summary>
        /// <param name="q">Quaternion representing a rotation</param>
        /// <param name="v">Vector to rotate</param>
        /// <returns>Rotation result q^-1*v*q</returns>
        public static Vector3 Rotate(Quaternion q, Vector3 v)
        {
            Quaternion point = new Quaternion(v.X, v.Y, v.Z, 0.0);
            Quaternion result = q * point * Conjugate(q);
            return new Vector3(result.xcomponent, result.ycomponent, result.zcomponent);
        }

        /// <summary>
        /// The equals operator for reference types
        /// </summary>
        /// <param name="leftSide">Quaternion on the left side</param>
        /// <param name="rightSide">Quaternion on the right side</param>
        /// <returns>Are the Quaternion values equal</returns>
        public static bool operator ==(Quaternion leftSide, Quaternion rightSide)
        {
            // Return true if the fields match:
            return (Math.Abs(leftSide.X - rightSide.X) < MathConstants.ErrorEpsilon
                   && Math.Abs(leftSide.Y - rightSide.Y) < MathConstants.ErrorEpsilon
                   && Math.Abs(leftSide.Z - rightSide.Z) < MathConstants.ErrorEpsilon
                   && Math.Abs(leftSide.W - rightSide.W) < MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// The not equals operator for reference types
        /// </summary>
        /// <param name="leftSide">Vector on the left side</param>
        /// <param name="rightSide">Vector on the right side</param>
        /// <returns>Comparison result</returns>
        public static bool operator !=(Quaternion leftSide, Quaternion rightSide)
        {
            return !(leftSide == rightSide);
        }

        /// <summary>
        /// Retrieves a string representation of the Quaternion.
        /// </summary>
        /// <returns>String that represents the Quaternion.</returns>
        public override string ToString()
        {
            return string.Format("X:{0} Y:{1} Z:{2} W:{3}", this.X, this.Y, this.Z, this.W);
        }

        /// <summary>
        /// Comparison of two quaternions
        /// </summary>
        /// <param name="obj">Quaternion to be compared to</param>
        /// <returns>Comparison result</returns>
        public override bool Equals(object obj)
        {
            if (!(obj is Quaternion))
            {
                return false;
            }

            Quaternion q = (Quaternion)obj;
            return (Math.Abs(this.xcomponent - q.xcomponent) < MathConstants.ErrorEpsilon)
                   && (Math.Abs(this.ycomponent - q.ycomponent) < MathConstants.ErrorEpsilon)
                   && (Math.Abs(this.zcomponent - q.zcomponent) < MathConstants.ErrorEpsilon)
                   && (Math.Abs(this.wcomponent - q.wcomponent) < MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Returns HashCode of object.
        /// </summary>
        /// <returns>Hashcode result</returns>
        public override int GetHashCode()
        {
            return base.GetHashCode();
        }

        /// <summary>
        /// Returns 1 if value is zero, Math.Sign(value) otherwise.
        /// </summary>
        /// <param name="value">A numeric value.</param>
        /// <returns>1 if value == 0, Math.Sign(value) otherwise.</returns>
        private static double ZeroPosSign(double value)
        {
            if (value == 0.0)
            {
                return 1.0;
            }
            else
            {
                return Math.Sign(value);
            }
        }
    }
}
